import java.io.DataOutputStream;
import java.io.IOException;

import lejos.nxt.*;
import lejos.nxt.addon.*;
import lejos.nxt.comm.*;

/**
 * Class to take care of sensor data.
 * Is a singleton.
 */
public class BillManager {
	// sensors
	private static CompassSensor compass;
	private static IRSeeker infrared;
	private static LightSensor light;
	private static UltrasonicSensor ultrasonic;

	// sensor data
	private static float compassData;
	private static int infraredData;
	private static int infraredSensor1Data;
	private static int infraredSensor2Data;
	private static int infraredSensor3Data;
	private static int infraredSensor4Data;
	private static int infraredSensor5Data;
	private static int lightData;
	private static int ultrasonicData;

	// debug
	private static boolean debug = false;
	private static NXTConnection conn;
	private static DataOutputStream dos;
	private static int ticks = 0;

	// virtual sensor
	private static boolean hasBall = false ;
	private static boolean isFacingGoal = false;
	
	
	
	private static boolean isInitialized = false;
	private static BillManager bill;

	private BillManager() {
	}

	/**
	 * Checks if BillManager is initialized. If not, throw an exception.
	 */
	private static void checkIfReady() {
		if (!isInitialized) {
			try {
				throw new Exception("BillManager not initialized");
			} catch (Exception e) {
				e.printStackTrace();
			}
		}
	}

	/**
	 * @param compass
	 * @param infrared
	 * @param light
	 * @param ultrasonic
	 */
	public static void Initialize(SensorPort compassSensor,
			SensorPort infraredSensor, SensorPort lightSensor,
			SensorPort ultrasonicSensor, boolean enableDebug) {

		if (bill == null)
			bill = new BillManager();

		compass = new CompassSensor(compassSensor);
		infrared = new IRSeeker(infraredSensor);
		light = new LightSensor(lightSensor);
		ultrasonic = new UltrasonicSensor(ultrasonicSensor);

		compass.resetCartesianZero();

		if (enableDebug) {
			initializeDebug();
		}

		isInitialized = true;
	}

	/**
	 * Waits for a bluetooth connection with a timeout of 30 seconds before continuing.
	 */
	private static void initializeDebug() {
		debug = true;
		LCD.clear();
		System.out.println("Waiting for BT");
		conn = Bluetooth.waitForConnection(30000, NXTConnection.PACKET);
		
		if (conn == null) {
			System.out.println("BT connect failed");
			debug = false;
		} else {
			dos = conn.openDataOutputStream();
		}
	}

	/**
	 * Asks the sensors for the new (real) data.
	 */
	public static void LoadLiveData() {
		checkIfReady();
		compassData = compass.getDegreesCartesian();
		infraredData = infrared.getDirection();
		infraredSensor1Data = infrared.getSensorValue(1);
		infraredSensor2Data = infrared.getSensorValue(2);
		infraredSensor3Data = infrared.getSensorValue(3);
		infraredSensor4Data = infrared.getSensorValue(4);
		infraredSensor5Data = infrared.getSensorValue(5);
		lightData = light.getLightValue();
		
//		//REMOTE DEBUG MITCHELL
//		if(!lejos.nxt.comm.RConsole.isOpen())
//		{
//			lejos.nxt.comm.RConsole.openBluetooth(0);
//		}
//		String infraredValue = String.valueOf(infrared.getDirection());
//		String infraredValue1 = String.valueOf(infrared.getSensorValue(1));
//		String infraredValue2 = String.valueOf(infrared.getSensorValue(2));
//		String infraredValue3 = String.valueOf(infrared.getSensorValue(3));
//		String infraredValue4 = String.valueOf(infrared.getSensorValue(4));
//		String infraredValue5 = String.valueOf(infrared.getSensorValue(5));
//		lejos.nxt.comm.RConsole.print(infraredValue1 + ",");
//		lejos.nxt.comm.RConsole.print(infraredValue2 + ",");
//		lejos.nxt.comm.RConsole.print(infraredValue3 + ",");
//		lejos.nxt.comm.RConsole.print(infraredValue4 + ",");
//		lejos.nxt.comm.RConsole.print(infraredValue5);
//		lejos.nxt.comm.RConsole.println("");
//		//END REMOTE DEBUG MITCHELL
		
		ultrasonicData = ultrasonic.getDistance();
		ticks++;

		if (debug) {
			SendDataBT();
		}
	}

	/**
	 * Sends the sensor data.
	 */
	public static void SendDataBT() {
		send(ticks);
		send((int)compassData);
		send(infraredData);
		send(infraredSensor3Data);
		send(lightData);
		send(ultrasonicData);
		//send(255*255);
	}

	/**
	 * Sends an integer to the DataOutputStream
	 * @param i Number to send.
	 */
	private static void send(int i) {
		try {
			dos.write(i >> 8);
			dos.write(i - (i >> 8) * 256);
			dos.flush();
		} catch (IOException e) {
			debug = false;
		}
	}

	/**
	 * Gets the compass data.
	 * @return Compass data.
	 */
	public static float getCompassData() {
		checkIfReady();
		return compassData;
	}

	/**
	 * Gets the infrared data.
	 * @return Infrared data.
	 */
	public static int GetInfraredData() {
		checkIfReady();
		return infraredData;
	}
	
	public static int GetInfraredSensor1Data() {
		checkIfReady();
		return infraredSensor1Data;
	}
	
	public static int GetInfraredSensor2Data() {
		checkIfReady();
		return infraredSensor2Data;
	}
	
	public static int GetInfraredSensor3Data() {
		checkIfReady();
		return infraredSensor3Data;
	}
	
	public static int GetInfraredSensor4Data() {
		checkIfReady();
		return infraredSensor4Data;
	}
	
	public static int GetInfraredSensor5Data() {
		checkIfReady();
		return infraredSensor5Data;
	}

	/**
	 * Gets the light data.
	 * @return Light data.
	 */
	public static int GetLightData() {
		checkIfReady();
		return lightData;
	}

	/**
	 * Get the ultrasonic data.
	 * @return Ultrasonic data.
	 */
	public static int GetUltrasonicData() {
		checkIfReady();
		return ultrasonicData;
	}
	
	/**
	 * Get the HasBall.
	 * @return HasBall.
	 */
	public static boolean HasBall()
	{
		return hasBall;
	}
	/**
	 * Sets the HasBall.
	 */
	public static void SetHasBall(boolean _hasBall)
	{
		hasBall = _hasBall;
	}
	

	
	public static boolean IsFacingGoal()
	{
		return isFacingGoal;
	}
	public static void SetIsFacingGoal(boolean _isFacingGoal)
	{
		isFacingGoal = _isFacingGoal;
	}
	
	
	
}
